#include "Drivers/CMSIS/Device/ST/STM32F4xx/Include/stm32f4xx.h" // Device header
#include "stm32f4xx_conf.h"
#include "StepMotor.h"

/* 电机实例定义 */
StepMotor_t StepMotor1 = {
    .tim = TIM4,
    .step_pin = GPIO_Pin_12,
    .step_port = GPIOD,
    .dir_pin = GPIO_Pin_13,
    .dir_port = GPIOD,
    .frequency = 1000,
    .max_freq = 5000,
    .remaining_steps = 0,
    .state = STEPMOTOR_IDLE
};

StepMotor_t StepMotor2 = {
    .tim = TIM5,
    .step_pin = GPIO_Pin_2,     // PA2作为STEP引脚
    .step_port = GPIOA,         // GPIOA端口
    .dir_pin = GPIO_Pin_15,     // DIR引脚PD15
    .dir_port = GPIOD,
    .frequency = 1000,
    .max_freq = 5000,
    .remaining_steps = 0,
    .state = STEPMOTOR_IDLE
};

/* 电机初始化函数 */
void StepMotor_Init(StepMotor_t* stepmotor) {
    // 使能时钟
    RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOD | RCC_AHB1Periph_GPIOA, ENABLE);
    if (stepmotor->tim == TIM4) {
        RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4, ENABLE);
    } else if (stepmotor->tim == TIM5) {
        RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM5, ENABLE);
    }

    // 配置DIR引脚
    GPIO_InitTypeDef GPIO_InitStructure;
    GPIO_InitStructure.GPIO_Pin = stepmotor->dir_pin;
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT;
    GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
    GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_DOWN;
    GPIO_Init(stepmotor->dir_port, &GPIO_InitStructure);
    GPIO_ResetBits(stepmotor->dir_port, stepmotor->dir_pin);  // 默认正方向

    // 配置STEP引脚为复用功能
    GPIO_InitStructure.GPIO_Pin = stepmotor->step_pin;
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz;
    GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP;
    GPIO_Init(stepmotor->step_port, &GPIO_InitStructure);

    // 配置复用为定时器
    if (stepmotor->tim == TIM4) {
        GPIO_PinAFConfig(stepmotor->step_port, GPIO_PinSource12, GPIO_AF_TIM4);
    } else if (stepmotor->tim == TIM5) {
        GPIO_PinAFConfig(stepmotor->step_port, GPIO_PinSource2, GPIO_AF_TIM5);  // PA2复用为TIM5_CH3
    }

    // 定时器基础配置
    TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
    uint32_t arr = (8400000 / stepmotor->frequency) - 1;
    TIM_TimeBaseStructure.TIM_Period = arr;
    TIM_TimeBaseStructure.TIM_Prescaler = 10-1;
    TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1;
    TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
    TIM_TimeBaseInit(stepmotor->tim, &TIM_TimeBaseStructure);

    // PWM配置（50%占空比）
    TIM_OCInitTypeDef TIM_OCInitStructure;
    TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;
    TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
    TIM_OCInitStructure.TIM_Pulse = arr/2 - 1;
    TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;
    
    if (stepmotor->tim == TIM4) {
        TIM_OC1Init(stepmotor->tim, &TIM_OCInitStructure);  // TIM4_CH1
        TIM_OC1PreloadConfig(stepmotor->tim, TIM_OCPreload_Enable);
    } else if (stepmotor->tim == TIM5) {
        TIM_OC3Init(stepmotor->tim, &TIM_OCInitStructure);  // TIM5_CH3 (PA2)
        TIM_OC3PreloadConfig(stepmotor->tim, TIM_OCPreload_Enable);
    }

    // 配置中断
    NVIC_InitTypeDef NVIC_InitStructure;
    if (stepmotor->tim == TIM4) {
        NVIC_InitStructure.NVIC_IRQChannel = TIM4_IRQn;
    } else if (stepmotor->tim == TIM5) {
        NVIC_InitStructure.NVIC_IRQChannel = TIM5_IRQn;
    }
    NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1;
    NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
    NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
    NVIC_Init(&NVIC_InitStructure);

    // 使能更新中断
    TIM_ITConfig(stepmotor->tim, TIM_IT_Update, ENABLE);

    // 初始禁用定时器
    TIM_Cmd(stepmotor->tim, DISABLE);
    TIM_CtrlPWMOutputs(stepmotor->tim, DISABLE);
}

/* 设置方向 */
void StepMotor_SetDirection(StepMotor_t* stepmotor, uint8_t dir) {
    if (dir==0) {
        GPIO_SetBits(stepmotor->dir_port, stepmotor->dir_pin);
    } else {
        GPIO_ResetBits(stepmotor->dir_port, stepmotor->dir_pin);
    }
}

/* 设置频率 */
void StepMotor_SetFrequency(StepMotor_t* stepmotor, uint32_t freq) {
    if (freq > 0 && freq <= stepmotor->max_freq) {
        stepmotor->frequency = freq;
        uint32_t arr = (8400000 / freq) - 1;
        TIM_SetAutoreload(stepmotor->tim, arr);
			if(stepmotor->tim==TIM4)
        TIM_SetCompare1(stepmotor->tim, arr/2 - 1);  // 保持50%占空比
			else
				TIM_SetCompare3(stepmotor->tim, arr/2 - 1);  // 保持50%占空比
    }
}

/* 移动指定步数 */
void StepMotor_MoveSteps(StepMotor_t* stepmotor, int32_t steps) {
    if (stepmotor->state == STEPMOTOR_IDLE && steps != 0) {
        // 设置方向
				if(steps>0){
					StepMotor_SetDirection(stepmotor, 1);
					stepmotor->remaining_steps = steps;
				}
				else{
					StepMotor_SetDirection(stepmotor, 0);
					stepmotor->remaining_steps = -steps;
				}
				stepmotor->state = STEPMOTOR_RUNNING;
        
        // 重置定时器并启动
        TIM_SetCounter(stepmotor->tim, 0);
        TIM_Cmd(stepmotor->tim, ENABLE);
        TIM_CtrlPWMOutputs(stepmotor->tim, ENABLE);
    }
}

/* 停止电机 */
void StepMotor_Stop(StepMotor_t* stepmotor) {
    if (stepmotor->state != STEPMOTOR_IDLE) {
        TIM_Cmd(stepmotor->tim, DISABLE);
        stepmotor->state = STEPMOTOR_IDLE;
        stepmotor->remaining_steps = 0;
    }
}

/* 暂停电机 */
void StepMotor_Pause(StepMotor_t* stepmotor) {
    if (stepmotor->state == STEPMOTOR_RUNNING) {
        TIM_Cmd(stepmotor->tim, DISABLE);
        stepmotor->state = STEPMOTOR_PAUSED;
    }
}

/* 恢复电机 */
void StepMotor_Resume(StepMotor_t* stepmotor) {
    if (stepmotor->state == STEPMOTOR_PAUSED) {
        TIM_Cmd(stepmotor->tim, ENABLE);
        stepmotor->state = STEPMOTOR_RUNNING;
    }
}

/* 获取电机状态 */
StepMotorState_t StepMotor_GetState(StepMotor_t* stepmotor) {
    return stepmotor->state;
}

/* 获取剩余步数 */
int32_t StepMotor_GetRemainingSteps(StepMotor_t* stepmotor) {
    return stepmotor->remaining_steps;
}

/* 定时器4中断（电机1） */
void TIM4_IRQHandler(void) {
    if (TIM_GetITStatus(TIM4, TIM_IT_Update) != RESET) {
        TIM_ClearITPendingBit(TIM4, TIM_IT_Update);
        if (StepMotor1.remaining_steps > 0) {
            StepMotor1.remaining_steps--;
            if (StepMotor1.remaining_steps == 0) {
                StepMotor1.state = STEPMOTOR_IDLE;
                TIM_Cmd(TIM4, DISABLE);
            }
        }
    }
}

/* 定时器5中断（电机2） */
void TIM5_IRQHandler(void) {
    if (TIM_GetITStatus(TIM5, TIM_IT_Update) != RESET) {
        TIM_ClearITPendingBit(TIM5, TIM_IT_Update);
        if (StepMotor2.remaining_steps > 0) {
            StepMotor2.remaining_steps--;
            if (StepMotor2.remaining_steps == 0) {
                StepMotor2.state = STEPMOTOR_IDLE;
                TIM_Cmd(TIM5, DISABLE);
            }
        }
    }
}

